/*
 * =====================================================================================
 *
 *       Filename:  cholesky.h
 *
 *    Description:  Cholesky Decomposition
 *
 *        Version:  1.0
 *        Created:  2009年06月11日 16时36分33秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Ying Wang (WY), ywang@nlpr.ia.ac.cn
 *        Company:  Institute of Automation, Chinese Academy of Sciences
 *
 * =====================================================================================
 */
#ifndef CHOLESKY_H
#define CHOLESKY_H
#include "ncvector.h"
#include "ncmatrix.h"
#include <cmath>
/**
 * Cholesky decomposition for symmetric and positive-definite matrix, A= L*L', where L is a lower triangle matrix.
 *
 */
class Cholesky
{
public:
	 inline explicit Cholesky(const NCmatrix<double> &a);

	/**
	 * \brief solve L*y = b
	 *
	 * \param y
	 *
	 * \return return the result b
	 */
	inline NCvector<double>  elmult(const NCvector<double> &y) const;

	/**
	 * \brief solve L*y = b, solution is y
	 *
	 * \param b
	 *
	 * \return return the solution y
	 */
	inline NCvector<double> elsolve(const NCvector<double> &b) const;

	/**
	 * \brief solve the inverse matrix of A
	 *
	 * \return the inverse matrix
	 */
	 inline NCmatrix<double> inverse() const;

	/**
	 * \brief return the log determinant of matrix A
	 *
	 * \return return the log determinant.
	 */
	inline  double logdet() const;
private:

	NCmatrix<double> LU;
    int row;
};


inline  Cholesky :: Cholesky(const NCmatrix<double> &a)
	: LU(a)
	, row(a.row())
{
	int i,j,k;
	NCvector<double> tmp;
	double sum;
	if(LU.column() != row)
	{
		std::cerr<<"Cholesky nedd a square matrix"<<std::endl;
		throw ("Need a square matrix");
	}
	for( i=0; i<row; i++)
	{
		for ( j=0; j<row; j++)
		{
			for ( sum=LU[i][j],k=i-1; k>=0;k-- )
				sum -= LU[i][k]*LU[j][k];
			if (i==j)
			{
				if (sum <= 0.0)
				{
					std::cerr<<"cholesky failed"<<std::endl;
					throw ("cholesky failed");
				}
				LU[i][i] = sqrt(sum);
			}
			else
				LU[j][i] = sum/LU[i][i];
		}
	}
	for ( i=0; i<row; i++)
		for ( j=0; j<i; j++ )
			LU[j][i] = 0;
}

inline NCvector<double> Cholesky :: elmult(const NCvector<double> &y) const
{
	int i,j;
	NCvector<double> b(row);
	if(y.size() != row || b.size() != row)
	{
		std::cerr<<"elmult dimension is not consistent"<<std::endl;
		throw("Dimension is not consistent");
	}
	for ( i=0; i<row; i++)
	{
		b[i] = 0;
		for ( j=0; j<=i; j++ )
		{
			b[i] += LU[i][j] * y[i];
		}
	}
	return b;
}

inline NCvector<double> Cholesky :: elsolve(const NCvector<double> &b) const
{
	int i,j;
	double sum;
	if ( b.size() != row)
	{
		std::cerr<<"elsolve dimension is not consistent"<<std::endl;
		throw ("Dimension is not consistent");
	}
	NCvector<double> y(row);
	for ( i=0; i<row; i++ )
	{
		for ( sum=b[i],j=0; j<i; j++ )
		{
			sum -= LU[i][j]*y[j];
		}
		y[i] = sum/LU[i][j];
	}
	return y;
}

inline NCmatrix<double> Cholesky :: inverse() const
{
	int i,j,k;
	double sum;
	NCmatrix<double> ainv(row,row);
	for ( i=0; i<row; i++)
	{
		for ( j=0; j<=i; j++ )
		{
			sum = (i==j ? 1. : 0.);
			for(k=i-1;k>=j;k--)
			{
				sum -= LU[i][k]*ainv[j][k];
			}
			ainv[j][i] = sum/LU[i][i];
		}
	}
	for ( i=row-1; i>=0; i-- )
	{
		for( j=0; j<=i; j++ )
		{
			sum = (i<j ? 0. : ainv[j][i]);
			for (k=i+1;k<row;k++)
			{
				sum -= LU[k][i] * ainv[j][k];
			}
			ainv[i][j] = ainv[j][i] = sum/LU[i][i];
		}
	}
	return ainv;
}

inline double Cholesky :: logdet() const
{
	double sum = 0.;
	for( int i=0; i<row; i++ )
		sum += log(LU[i][i]);
	return 2.*sum;
}
#endif

